//
// Created by oftenlin on 23-9-11.
//

#ifndef LANGGE_VDR_SIMPLERTK_HPP
#define LANGGE_VDR_SIMPLERTK_HPP
#include "NaviConstant.h"
const static double gpst0[] = {1980, 1, 6, 0, 0, 0}; /* gps time reference */
//const static double gst0[] = {1999, 8, 22, 0, 0, 0}; /* galileo system time reference */
//const static double bdt0[] = {2006, 1, 1, 0, 0, 0}; /* beidou time reference */

typedef struct {        /* time struct */
    time_t time;        /* time (s) expressed by standard time_t */
    double sec;         /* fraction of second under 1 s */
} gtime_t;

class simpleRtk {
public:
    /* time to calendar day/time ---------------------------------------------------
    * convert gtime_t struct to calendar day/time
    * args   : gtime_t t        I   gtime_t struct
    *          double *ep       O   day/time {year,month,day,hour,min,sec}
    * return : none
    * notes  : proper in 1970-2037 or 1970-2099 (64bit time_t)
    *-----------------------------------------------------------------------------*/
    //    inline static void time2epoch(gtime_t t, double *ep) {
    //        const int mday[] = { /* # of days in a month */
    //                31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31,
    //                31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31
    //        };
    //        int days, sec, mon, day;
    //
    //        /* leap year if year%4==0 in 1901-2099 */
    //        days = (int) (t.time / 86400);
    //        sec = (int) (t.time - (time_t) days * 86400);
    //        for (day = days % 1461, mon = 0; mon < 48; mon++) {
    //            if (day >= mday[mon]) day -= mday[mon]; else break;
    //        }
    //        ep[0] = 1970 + days / 1461 * 4 + mon / 12;
    //        ep[1] = mon % 12 + 1;
    //        ep[2] = day + 1;
    //        ep[3] = sec / 3600;
    //        ep[4] = sec % 3600 / 60;
    //        ep[5] = sec % 60 + t.sec;
    //    }

    /* convert calendar day/time to time -------------------------------------------
    * convert calendar day/time to gtime_t struct
    * args   : double *ep       I   day/time {year,month,day,hour,min,sec}
    * return : gtime_t struct
    * notes  : proper in 1970-2037 or 1970-2099 (64bit time_t)
    *-----------------------------------------------------------------------------*/
    inline static gtime_t epoch2time(const double *ep) {
        const int doy[] = {1, 32, 60, 91, 121, 152, 182, 213, 244, 274, 305, 335};
        gtime_t time = {0};
        int days, sec, year = (int) ep[0], mon = (int) ep[1], day = (int) ep[2];

        if (year < 1970 || 2099 < year || mon < 1 || 12 < mon) return time;

        /* leap year if year%4==0 in 1901-2099 */
        days = (year - 1970) * 365 + (year - 1969) / 4 + doy[mon - 1] + day - 2 + (year % 4 == 0 && mon >= 3 ? 1 : 0);
        sec = (int) floor(ep[5]);
        time.time = (time_t) days * 86400 + (int) ep[3] * 3600 + (int) ep[4] * 60 + sec;
        time.sec = ep[5] - sec;
        return time;
    }

    inline static double time2gpst(gtime_t t, int *week) {
        gtime_t t0 = epoch2time(gpst0);
        time_t sec = t.time - t0.time;
        int w = (int) (sec / (86400 * 7));

        if (week) *week = w;
        return (double) (sec - w * 86400 * 7) + t.sec;
    }

    /* galileo system time to time -------------------------------------------------
    * convert week and tow in galileo system time (gst) to gtime_t struct
    * args   : int    week      I   week number in gst
    *          double sec       I   time of week in gst (s)
    * return : gtime_t struct
    *-----------------------------------------------------------------------------*/
    //    inline static gtime_t gst2time(int week, double sec) {
    //        gtime_t t = epoch2time(gst0);
    //        if (sec < -1E9 || 1E9 < sec) sec = 0.0;
    //        t.time += 86400 * 7 * week + (int) sec;
    //        t.sec = sec - (int) sec;
    //        return t;
    //    }

    /* add time --------------------------------------------------------------------
    * add time to gtime_t struct
    * args   : gtime_t t        I   gtime_t struct
    *          double sec       I   time to add (s)
    * return : gtime_t struct (t+sec)
    *-----------------------------------------------------------------------------*/
    inline static gtime_t timeadd(gtime_t t, double sec) {
        double tt;

        t.sec += sec;
        tt = floor(t.sec);
        t.time += (int) tt;
        t.sec -= tt;
        return t;
    }

    /* time difference -------------------------------------------------------------
    * difference between gtime_t structs
    * args   : gtime_t t1,t2    I   gtime_t structs
    * return : time difference (t1-t2) (s)
    *-----------------------------------------------------------------------------*/
    inline static double timediff(gtime_t t1, gtime_t t2) {
        return difftime(t1.time, t2.time) + t1.sec - t2.sec;
    }

    /* transform geodetic to ecef position -----------------------------------------
    * transform geodetic position to ecef position
    * args   : double *pos      I   geodetic position {lat,lon,h} (rad,m)
    *          double *r        O   ecef position {x,y,z} (m)
    * return : none
    * notes  : WGS84, ellipsoidal height
    *-----------------------------------------------------------------------------*/
    inline static void pos2ecef(const double *pos, double *r) {

        double sinp = sin(pos[0]), cosp = cos(pos[0]), sinl = sin(pos[1]), cosl = cos(pos[1]);
        double e2 = FE_WGS84 * (2.0 - FE_WGS84), v = RE_WGS84 / sqrt(1.0 - e2 * sinp * sinp);

        r[0] = (v + pos[2]) * cosp * cosl;
        r[1] = (v + pos[2]) * cosp * sinl;
        r[2] = (v * (1.0 - e2) + pos[2]) * sinp;
    }

    /* ecef to local coordinate transfromation matrix ------------------------------
    * compute ecef to local coordinate transfromation matrix
    * args   : double *pos      I   geodetic position {lat,lon} (rad)
    *          double *E        O   ecef to local coord transformation matrix (3x3)
    * return : none
    * notes  : matirix stored by column-major order (fortran convention)
    *-----------------------------------------------------------------------------*/
    //    inline static void xyz2enu(const double *pos, double *E) {
    //        double sinp = sin(pos[0]), cosp = cos(pos[0]), sinl = sin(pos[1]), cosl = cos(pos[1]);
    //
    //        E[0] = -sinl;
    //        E[3] = cosl;
    //        E[6] = 0.0;
    //        E[1] = -sinp * cosl;
    //        E[4] = -sinp * sinl;
    //        E[7] = cosp;
    //        E[2] = cosp * cosl;
    //        E[5] = cosp * sinl;
    //        E[8] = sinp;
    //    }

    /* transform ecef vector to local tangental coordinate -------------------------
    * transform ecef vector to local tangental coordinate
    * args   : double *pos      I   geodetic position {lat,lon} (rad)
    *          double *r        I   vector in ecef coordinate {x,y,z}
    *          double *e        O   vector in local tangental coordinate {e,n,u}
    * return : none
    *-----------------------------------------------------------------------------*/
    //    inline static void ecef2enu(const double *pos, const double *r, double *e) {
    //        double E[9];
    //        xyz2enu(pos, E);
    //        matmul("NN", 3, 1, 3, 1.0, E, r, 0.0, e);
    //    }

    /* transform local vector to ecef coordinate -----------------------------------
    * transform local tangental coordinate vector to ecef
    * args   : double *pos      I   geodetic position {lat,lon} (rad)
    *          double *e        I   vector in local tangental coordinate {e,n,u}
    *          double *r        O   vector in ecef coordinate {x,y,z}
    * return : none
    *-----------------------------------------------------------------------------*/
    //    inline void enu2ecef(const double *pos, const double *e, double *r) {
    //        double E[9];
    //        xyz2enu(pos, E);
    //        matmul("TN", 3, 1, 3, 1.0, E, e, 0.0, r);
    //    }

    /* multiply matrix -----------------------------------------------------------*/
    //    inline static void matmul(const char *tr, int n, int k, int m, double alpha,
    //                              const double *A, const double *B, double beta, double *C) {
    //        double d;
    //        int i, j, x, f = tr[0] == 'N' ? (tr[1] == 'N' ? 1 : 2) : (tr[1] == 'N' ? 3 : 4);
    //
    //        for (i = 0; i < n; i++)
    //            for (j = 0; j < k; j++) {
    //                d = 0.0;
    //                switch (f) {
    //                    case 1:
    //                        for (x = 0; x < m; x++) d += A[i + x * n] * B[x + j * m];
    //                        break;
    //                    case 2:
    //                        for (x = 0; x < m; x++) d += A[i + x * n] * B[j + x * k];
    //                        break;
    //                    case 3:
    //                        for (x = 0; x < m; x++) d += A[x + i * m] * B[x + j * m];
    //                        break;
    //                    case 4:
    //                        for (x = 0; x < m; x++) d += A[x + i * m] * B[j + x * k];
    //                        break;
    //                }
    //                if (beta == 0.0) C[i + j * n] = alpha * d; else C[i + j * n] = alpha * d + beta * C[i + j * n];
    //            }
    //    }
    // 计算周秒数转时间戳
    //    inline static gtime_t simpleRtk::gpst2time(int week, double sec) {
    //        gtime_t t = epoch2time(gpst0);
    //
    //        if (sec < -1E9 || 1E9 < sec) sec = 0.0;
    //        t.time += 86400 * 7 * week + (int) sec;
    //        t.sec = sec - (int) sec;
    //        return t;
    //    }

    /* transform covariance to local tangental coordinate --------------------------
    * transform ecef covariance to local tangental coordinate
    * args   : double *pos      I   geodetic position {lat,lon} (rad)
    *          double *P        I   covariance in ecef coordinate
    *          double *Q        O   covariance in local tangental coordinate
    * return : none
    *-----------------------------------------------------------------------------*/
    //    inline void covenu(const double *pos, const double *P, double *Q) {
    //        double E[9], EP[9];
    //
    //        xyz2enu(pos, E);
    //        matmul("NN", 3, 3, 3, 1.0, E, P, 0.0, EP);
    //        matmul("NT", 3, 3, 3, 1.0, EP, E, 0.0, Q);
    //    }


    /* transform local enu coordinate covariance to xyz-ecef -----------------------
    * transform local enu covariance to xyz-ecef coordinate
    * args   : double *pos      I   geodetic position {lat,lon} (rad)
    *          double *Q        I   covariance in local enu coordinate
    *          double *P        O   covariance in xyz-ecef coordinate
    * return : none
    *-----------------------------------------------------------------------------*/
    //    inline void covecef(const double *pos, const double *Q, double *P) {
    //        double E[9], EQ[9];
    //
    //        xyz2enu(pos, E);
    //        matmul("TN", 3, 3, 3, 1.0, E, Q, 0.0, EQ);
    //        matmul("NN", 3, 3, 3, 1.0, EQ, E, 0.0, P);
    //    }

    // 计算 矩阵序列均值
    //    inline static Eigen::Vector3d
    //    calMeanUsingSeq(Eigen::Vector3d lastest_data, Eigen::Vector3d oldest_data, Eigen::Vector3d pre_mean, int vec_len) {
    //        return pre_mean + (1.0 / vec_len) * (lastest_data - oldest_data);
    //    }
    // 计算 矩阵序列标准差
    //    inline static double
    //    calStdUsingSeq(Eigen::Vector3d lastest_data, Eigen::Vector3d oldest_data, double pre_std, Eigen::Vector3d pre_mean,
    //                   Eigen::Vector3d cur_mean, int vec_len) {
    //        return pre_std + ((cur_mean - pre_mean) * (1.0 / vec_len)).transpose() *
    //                         ((lastest_data - oldest_data) * (vec_len - 1) - 2 * vec_len * pre_mean + 2 * oldest_data);
    //    }
};
#endif //LANGGE_VDR_SIMPLERTK_HPP
